_setVecHelper = function(vec, x,y,z) {
    if (x instanceof Vec) {
	vec.x = x.x; vec.y = x.y; vec.z = x.z;
    }
    else {
	vec.x = x; vec.y = y; vec.z = z;
    }
    return vec;
};

Vec = function(x, y, z) {
    if (!(this instanceof Vec))
	return new Vec(x, y, z);
    _setVecHelper(this, x, y, z);
};
Vec.prototype.set = function(x, y, z) {
    return _setVecHelper(this, x, y, z);
};
Vec.prototype.clone = function() {
    return _setVecHelper(new Vec, this);  
};
Vec.prototype.scale = function(scalar) {
    return Vec(this).scaleMe(scalar);
};
Vec.prototype.add = function(other) {
    return Vec(this).addMe(other);
};
Vec.prototype.sub = function(other) {
    return Vec(this).subMe(other);
};
Vec.prototype.lengthSquared = function() {
    return this.x*this.x +
	   this.y*this.y +
	   this.z*this.z;
};
Vec.prototype.length = function() {
    return Math.sqrt(this.x*this.x +
		     this.y*this.y +
		     this.z*this.z);
};
Vec.prototype.cross = function(other) {
    return Vec(this.y*other.z - this.z*other.y,
	       this.z*other.x - this.x*other.z,
	       this.x*other.y - this.y*other.x);
};
Vec.prototype.dot = function(other) {
    return this.x*other.x + this.y*other.y + this.z*other.z;
};
Vec.prototype.addMe = function(other) {
    this.x += other.x;
    this.y += other.y;
    this.z += other.z;
    return this;
};
Vec.prototype.subMe = function(other) {
    this.x -= other.x;
    this.y -= other.y;
    this.z -= other.z;
    return this;
};
Vec.prototype.scaleMe = function(scalar) {
    this.x *= scalar;
    this.y *= scalar;
    this.z *= scalar;
    return this;
};
Vec.prototype.normalizeMe = function() {
    return this.scaleMe(1/this.length());
};

Point = function(posx, posy, posz) {
    if (!(this instanceof Point))
	return new Point(posx, posy, posz);
    this.pos = Vec(posx,posy,posz);
    this.vel = Vec(0,0,0);
    this.acc = Vec(0,0,0);
    this.groundContact = false;
    this.inHollowSolid = false;
};

Point.prototype.clone = function() {
    var clone = new Point(this.pos.x, this.pos.y, this.pos.z);
    clone.vel = this.vel.clone();
    clone.acc = this.acc.clone();
    clone.groundContact = this.groundContact;
    clone.inHollowSolid = this.inHollowSolid;
    return clone;
};

Constraint = function(pointIndex0, pointIndex1, distance) {
    if (!(this instanceof Constraint))
	return new Constraint(pointIndex0, pointIndex1, distance);

    this.index0 = pointIndex0;
    this.index1 = pointIndex1;
    this.distance = distance;
};


Constraint.prototype.clone = function() {
    // return this; Immutable???
    return new Constraint(this.index0, this.index1, this.distance);
};

PhyObj = function(points, constraints, sides) {
    if (!(this instanceof PhyObj))
	   return new PhyObj(points, constraints, sides);

    this.points = points;
    this.constraints = constraints;
    this.sides = sides; // [ [pointindex1, ..., pointindex4] ]
    this.faces = []; // [ {'pos':position, 'normal':normal} ]

    var s, normal, v1, v2;
    for (var i = 0; i < sides.length; i++) {
    	s = sides[i];
    	v1 = points[s[1]].pos.sub(points[s[0]].pos);
    	v2 = points[s[2]].pos.sub(points[s[1]].pos);
    	var normal = v1.cross(v2);
    	normal.normalizeMe();
    	this.faces.push( {'pos':points[s[0]].pos, 'normal':normal });
    }
};

PhyObj.prototype = {

    satisfyConstraints : function() {

        for (var j = 0; j < this.constraints.length; j++) {

            var c = this.constraints[j];
            var p0 = this.points[c.index0];
            var p1 = this.points[c.index1];
            var diff = p1.pos.sub(p0.pos);
            var dist = diff.length();
            var dir = diff.scale(1/dist);
            var corr = dir.scale((dist - c.distance)/5); // magic 5
            p0.vel = p0.vel.add(corr);
            p0.pos = p0.pos.add(corr);
            p1.vel = p1.vel.sub(corr);
            p1.pos = p1.pos.sub(corr);

        }

    },

    verletIntegrate : function(dt) {

        // Assume this is object is not moving if not having any constraints
        if (this.constraints.length == 0 && !this.moving)
            return;

        var p, newpos, points = this.points;
        for (var j = 0; j < points.length; j++) {

            p = points[j];
            newpos = p.pos.add(p.vel).add(p.acc.scale(dt*dt));
            p.vel.set( newpos.sub(p.pos) );
            p.pos.set( newpos );

        }

    },

    isSolid : function() {

        // Function may be overridden. Not so nice ...

        return false;

    },

    eachPoint : function(callback) {

        for (var i = 0; i < this.points.length; i++) {
            callback( this.points[i], i );
        };

    },

    testCollision : function(pos, onCollision, debuggga) {

        var face, dist; 
        var minDistIx = -1;
        var minDist = 10000000;
	
        for (var m = 0; m < this.faces.length; m++) {
	    
            face = this.faces[m];

            dist = pos.sub(face.pos).dot(face.normal);

            if (dist > 0)
                // There is no collision
                return;
        
            if (-dist < minDist &&
                m < 4) {   // Do not adjust to glue of solids
                minDist = -dist;
                minDistIx = m;
            }
        }
	
        // There must be a collision
        onCollision(this.faces[minDistIx].normal, minDist);
	
    },

    closestPos : function(pos) {

        // Will seldom find the exact closest point

        var ret = Vec(pos);
        var fs = this.faces;

        var iterate = function(p) {

            for (var m = 0; m < fs.length; m++) {

                face = fs[m];
                dist = ret.sub(face.pos).dot(face.normal);
                
                if (dist > 0)
                    p.subMe( face.normal.scale(dist) );                
            }

        };

        iterate(ret);
        iterate(ret);        

        return ret;

    },

    clone : function() {

        var i, n, points = [], constraints = [];
        
        for (i = 0, n = this.points.length; i < n; ++i) {
    	   points.push(this.points[i].clone());
        }
        for (i = 0, n = this.constraints.length; i < n; ++i) {
    	   constraints.push(this.constraints[i].clone());
        }
        return new PhyObj(points, constraints, this.sides);
    }
};





